#include "Vehicle.h"
#include "Road.h"
#include "GameManager.h"
#include <cmath>

#define X_TRAFFIC_STOP_PADDING 1*METTER_TO_POINT

Vehicle::Vehicle()
{
	vehicleId = GameManager::getInstance()->getCurrentVehicleId();
	numTurn = 0;
}

Vehicle::~Vehicle()
{
}

bool Vehicle::init(){
	GameObject::init();
	//scheduleUpdate();
	scheduleUpdateWithPriority(2);

	isSafe = true;
	horizontalDirection = HD_NONE;
	isEnableTurn = true;

	isInLane1 = isInLane2 = isInLane3 = false;
    
#if LOG_LABEL_ENABLE
    logLabel = CCLabelTTF::create("", "", 16, CCSizeMake(200, 30), kCCTextAlignmentCenter, kCCVerticalTextAlignmentCenter);
    updateLogString();
    logLabel->setPosition(ccp(0, -20));
    addChild(logLabel);
#endif

	return true;
}

void Vehicle::update(float delta){

	if(delta > TIME_NOT_UPDATE)
		return;
	
	calculateSafeRect();
	checkSafeWithAheadVehicles(oldPosition);
	checkTrafficLight();
    
#if LOG_LABEL_ENABLE
    updateLogString();
#endif
}

void Vehicle::updatePosition(float delta){
    
    if(state == kVehicleStateRunning) {
        speedUp();
        
        if(v.x >= expectV) {
            v.x = expectV;
            a.x = 0;
        }
    }
    
	oldPosition = getPosition();

    v.x += a.x*delta;
    v.y += a.y*delta;
    
    // if braking, stop when vehicle begin move backward
    if(state == kVehicleStateBraking) {
        if(v.x < 0) {
            v.x = 0;
            a.x = 0;
        }
    }
    
	float x = oldPosition.x + v.x*delta + 0.5*a.x*delta*delta;
	float y = oldPosition.y + v.y*delta + 0.5*a.y*delta*delta;

	setPosition(x, y);
}

void Vehicle::setRoad(Road* road){
	this->road = road;
}

CCRect Vehicle::getSafeRect(){
	return safeRectAhead;
}

CCPoint Vehicle::getV(){
	return v;
}

void Vehicle::checkTurnDone(){
	switch (horizontalDirection)
	{
	case HD_NONE:
		break;

	case HD_LEFT:
		if(getPositionY() > horizontalTarget){
			setPositionY(horizontalTarget);
			noTurn();
		}
		break;

	case HD_RIGHT:
		if(getPositionY() < horizontalTarget){
			setPositionY(horizontalTarget);
			noTurn();
		}
		break;
	}
}

void Vehicle::noTurn(){
	horizontalDirection = HD_NONE;
	v.y = 0;
	leftSafeRectBeside = CCRectZero;
	rightSafeRectBeside = CCRectZero;
}

void Vehicle::checkResignRoadForBus(Vehicle* vehicle){
	//if vehicle is bus
	if(vehicle != this && vehicle->getTag() == ID_BUS && getTag() != ID_BUS){

		// rectToCheck is boundingBox subtract some pixel, unless it not collision
		CCRect rectToCheck = CCRectMake(boundingBox().origin.x - 1*METTER_TO_POINT, boundingBox().origin.y, boundingBox().size.width, boundingBox().size.height);
		if(vehicle->getSafeRect().intersectsRect(rectToCheck)){
			isSafe = false;
		}
	}
}

void Vehicle::checkSafeWithAheadVehicles(CCPoint oldPosition){

	// check if in unsafe with ahead vehicle
	isSafe = true;
	float vxNew = INT_MAX;

	CCObject* it;
	CCARRAY_FOREACH(road->getVehicles(), it){
		Vehicle* vehicle = (Vehicle*)it;
		if(vehicle != this && isInInteractableRange(vehicle)){

			// check to resign road for bus
			checkResignRoadForBus(vehicle);

			// if I not in safe distance with ahead vehicle, and I run faster, I will slow down my speed, equal ahead vehicle
			if(getPositionX() < vehicle->getPosition().x && v.x >= vehicle->v.x){
                
                CCRect safeRectAheadNow = safeRectAhead;
                
                // is waiting red light
                if(vehicle->v.x == 0 || vehicle->getState() == kVehicleStateBraking) {
                    safeRectAheadNow = safeRectAheadWaitRedLight;
                }
				
				if(safeRectAheadNow.intersectsRect(vehicle->boundingBox())){
					if(vehicle->v.x < vxNew){
						vxNew = vehicle->v.x;
					}
				}
			}

			if(boundingBox().intersectsRect(vehicle->boundingBox()) && getPositionX() < vehicle->getPositionX()){
				CCPoint currentPosition = getPosition();
				setPosition(oldPosition);
			}
		}
	}

	if(vxNew != INT_MAX){
		v.x = vxNew;
		isSafe = false;
	}
}

void Vehicle::calculateSafeRect(){
	int safeDistanceAhead = getSafeDistanceAhead();
	int safeDistanceBeside = GameManager::getInstance()->safeDistanceBeside;

	safeRectAhead = CCRectMake(boundingBox().getMinX() - safeDistanceAhead, 
		boundingBox().getMinY(), 
		boundingBox().size.width + 2*safeDistanceAhead, 
		boundingBox().size.height);
	safeRectBeside = CCRectMake(boundingBox().getMidX(), 
		boundingBox().getMinY() - safeDistanceBeside, 
		2, 
		boundingBox().size.height + 2*safeDistanceBeside);

	leftSafeRectNextLane = CCRectMake(safeRectAhead.origin.x, safeRectAhead.origin.y + road->getLaneWidth(), safeRectAhead.size.width, safeRectAhead.size.height);
	rightSafeRectNextLane = CCRectMake(safeRectAhead.origin.x, safeRectAhead.origin.y - road->getLaneWidth(), safeRectAhead.size.width, safeRectAhead.size.height);
    
    safeRectAheadWaitRedLight = CCRectMake(boundingBox().getMinX() - safeDistanceAhead,
                               boundingBox().getMinY(),
                               boundingBox().size.width + safeDistanceAhead + distanceWhenWaitingRedLight,
                               boundingBox().size.height);
}

void Vehicle::checkTrafficLight(){
    if(road->getIsGreenLightOn() == false){
        float distanceToStop = getDistanceToStopWhenRedLight();
        if(getPositionX() > distanceToStop) {
            brake(-kG*kK);
        }
    }

	if(road->getIsGreenLightOn() && v.x == 0){
        speedUp();
	}
}

float Vehicle::calculateVTurn(){
	float vTurn = v.x/2;
	if(vTurn > 100)
		vTurn = 100;
	return vTurn;
}

CCRect Vehicle::getLeftSafeRectBeside(){
	return leftSafeRectBeside;
}

CCRect Vehicle::getRightSafeRectBeside(){
	return rightSafeRectBeside;
}
CCRect Vehicle::getLeftSafeRectNextLane(){
	return leftSafeRectNextLane;
}
CCRect Vehicle::getRightSafeRectNextLane(){
	return rightSafeRectNextLane;
}

CCRect Vehicle::getSafeRectBeside(){
	return safeRectBeside;
}

bool Vehicle::isInInteractableRange(Vehicle* vehicle){
	return abs(getPositionX() - vehicle->getPositionX()) < 25*METTER_TO_POINT;
}

int Vehicle::getVehicleId(){
	return vehicleId;
}

int Vehicle::getNumTurn(){
	return numTurn;
}

bool Vehicle::isBesideSafe(){
	return true;

	CCObject* it;
	CCARRAY_FOREACH(road->getVehicles(), it){
		Vehicle* vehicle = (Vehicle*)it;
		if(isInInteractableRange(vehicle) && safeRectBeside.intersectsRect(vehicle->getSafeRectBeside()) && vehicle != this){
			/*CCLog("isBesideSafe = false");
			CCLog("safeRectBeside %f, %f, %f, %f", safeRectBeside.origin.x, safeRectBeside.origin.y, safeRectBeside.size.width, safeRectBeside.size.height);
			CCLog("boundingBox %f, %f, %f, %f", boundingBox().origin.x, boundingBox().origin.y, boundingBox().size.width, boundingBox().size.height);*/
			return false;
		}
	}
	return true;
}

void Vehicle::calculateSafeRectBeside(){
	int safeDistanceBeside = GameManager::getInstance()->safeDistanceBeside;

	CCObject* it;
	CCARRAY_FOREACH(road->getVehicles(), it){
		Vehicle* vehicle = (Vehicle*)it;
		if(vehicle != this && isInInteractableRange(vehicle)){
			// if I not in safe distance with ahead vehicle, and I run faster, I will slow down my speed, equal ahead vehicle
			if(getPositionX() < vehicle->getPosition().x && v.x >= vehicle->v.x){

				if(safeRectAhead.intersectsRect(vehicle->boundingBox())){
					// calculate safeRectBeside
					if(leftSafeRectBeside.equals(CCRectZero))
						leftSafeRectBeside = CCRectMake(safeRectAhead.origin.x, vehicle->boundingBox().getMaxY() + safeDistanceBeside, safeRectAhead.size.width + 2*METTER_TO_POINT, safeRectAhead.size.height);

					if(rightSafeRectBeside.equals(CCRectZero))
						rightSafeRectBeside = CCRectMake(safeRectAhead.origin.x, vehicle->boundingBox().getMinY() - safeDistanceBeside - safeRectAhead.size.height, safeRectAhead.size.width + 2*METTER_TO_POINT, safeRectAhead.size.height);
					
				}
			}
		}
	}
}
void Vehicle::draw(){
	/*if(leftSafeRectBeside.equals(CCRectZero) == false){
		ccDrawRect(ccp(getLeftSafeRectBeside().getMinX() - getPositionX(), getLeftSafeRectBeside().getMinY() - getPositionY()), ccp(getLeftSafeRectBeside().getMaxX()- getPositionX(), getLeftSafeRectBeside().getMaxY()- getPositionY()));
		ccDrawRect(ccp(boundingBox().getMinX() - getPositionX(), boundingBox().getMinY() - getPositionY()), ccp(boundingBox().getMaxX()- getPositionX(), boundingBox().getMaxY()- getPositionY()));
	}*/
}

void Vehicle::readInfo(VehicleData* vehicleData) {
    expectV = vehicleData->expectV*METTER_TO_POINT;
    expectA = vehicleData->a*METTER_TO_POINT;
    expectDecreaseA = vehicleData->decreaseA*METTER_TO_POINT;
    distanceWhenWaitingRedLight = vehicleData->distanceWhenWaitingRedLight*METTER_TO_POINT;
    vChangeLane = vehicleData->vChangeLane*METTER_TO_POINT;
    vPassBus = vehicleData->vPassBus*METTER_TO_POINT;
    isChangeLane = vehicleData->isChangeLane;
    v.x = vehicleData->startV*METTER_TO_POINT;
}

float Vehicle::getSafeDistanceAhead() {
    return v.x*v.x/(2*9.8*0.7)*METTER_TO_POINT;
}

#if LOG_LABEL_ENABLE
void Vehicle::updateLogString() {
    sprintf(logString, "x: %.1f; v: %.1f; a: %.1f", getPositionX()*POINT_TO_METTER, v.x*POINT_TO_METTER, a.x*POINT_TO_METTER);
    logLabel->setString(logString);
}
#endif

void Vehicle::speedUp() {
    if(expectA != 0 && expectV != 0 && v.x < expectV) {
        a.x = expectA;
        changeState(kVehicleStateRunning);
    }
}

void Vehicle::changeState(int state) {
    GameObject::changeState(state);
    
    switch (state) {
        case kVehicleStateRunning:
            break;
            
        case kVehicleStateStop:
            break;
            
        case kVehicleStateBraking:
            break;
    }
    
}

void Vehicle::stop() {
    v.x = 0;
    a.x = 0;
    changeState(kVehicleStateStop);
}

Vehicle* Vehicle::getNearestVehicleAheadSameLane() {
    float minDistance = INT_MAX;
    int myLane = road->getWhichLane(this);
    Vehicle* nearestVehicleAhead = 0;
    CCObject* it;
    CCARRAY_FOREACH(road->getVehicles(), it){
        Vehicle* vehicle = (Vehicle*)it;
        int vehicleLane = road->getWhichLane(vehicle);
        if(vehicle != this && vehicle->getPositionX() > getPositionX() && vehicleLane == myLane && vehicle->getPositionX() < road->getContentSize().width && vehicle->v.x == 0){
            float distance = vehicle->getPositionX() - getPositionX();
            if(distance < minDistance) {
                minDistance = distance;
                nearestVehicleAhead = vehicle;
            }
        }
    }
    return nearestVehicleAhead;
}

float Vehicle::getDistanceToStopWhenRedLight() {
    float positionToStop = road->getContentSize().width - X_TRAFFIC_STOP_PADDING;
        Vehicle* nearestVehicle = getNearestVehicleAheadSameLane();
        if(nearestVehicle) {
            positionToStop = nearestVehicle->boundingBox().getMinX() - distanceWhenWaitingRedLight - boundingBox().size.width/2;
        }
    
    float distanceToStop = positionToStop - v.x*v.x/(2*kG*kK);
    return distanceToStop;
}


bool Vehicle::isWaitingRedLight() {
    if(v.x == 0 && a.x == 0 && road->getIsGreenLightOn() == false) {
        return true;
    }
    return false;
}

void Vehicle::brake(float fA) {
    a.x = fA;
    changeState(kVehicleStateBraking);
}